#ifndef INVERSE_KINEMATICS_H
#define INVERSE_KINEMATICS_H

#include <stdint.h>

/* 返回值 */
#define IK_SUCCESS 0
#define IK_ERR_OUT_OF_RANGE 1

/* 外部变量声明（需要在使用前定义） */
extern int ZERO_PULSE_THETA6;

/**
 * inverse_kinematics
 *
 * 基于 Matlab 版本的逆运动学（含工具/管尖参数 parameter）。
 *
 * 单位约定（非常重要）：
 *
 *  - parameter: 长度 6 的数组
 *      parameter[0..2] = xT0, yT0, zT0 (单位：μm)
 *      parameter[3..5] = alphaT0, betaT0, gammaT0 (单位：°)
 *
 *  - x, y, z: 末端目标位置（单位：μm）  <-- 已修正为 μm，与 Matlab 保持一致
 *  - rx, ry, rz: 末端目标姿态（单位：°）
 *
 * 输出：
 *  - pulse_* : 各轴脉冲（int）
 *
 * 返回：
 *  - IK_SUCCESS (0) 或 IK_ERR_OUT_OF_RANGE (1)
 */
int inverse_kinematics(const double parameter[6], double x_um, double y_um,
                       double z_um, double rx_deg, double ry_deg, double rz_deg,
                       int *pulse_theta1, int *pulse_d2, int *pulse_d3,
                       int *pulse_d4, int *pulse_theta5, int *pulse_theta6);

/**
 * @brief 增量式逆运动学计算
 *
 * @param parameter[6]     增量参数：[dx_um, dy_um, dz_um, dalpha_deg,
dbeta_deg, dgamma_deg]
 * @param parameter_FK[6]  当前绝对位置：[x_current_um, y_current_um,
z_current_um, alpha_current_deg, beta_current_deg, gamma_current_deg]
 * @param x_um            目标位置x坐标（微米）
 * @param y_um            目标位置y坐标（微米）
 * @param z_um            目标位置z坐标（微米）
 * @param rx_deg          绕x轴旋转角度（度）
 * @param ry_deg          绕y轴旋转角度（度）
*/

/**
 * incremental_inverse_kinematics
 *
 * 基于 Matlab 版本的增量式逆运动学。
 *
 * 单位约定（非常重要）：
 *
 *  - parameter: 增量参数，长度 6 的数组
 *      parameter[0..2] = dx, dy, dz (单位：μm) - 相对当前位置的增量
 *      parameter[3..5] = dalpha, dbeta, dgamma (单位：°) - 相对当前姿态的增量
 *
 *  - parameter_FK: 当前绝对位置，长度 6 的数组（通过正运动学获得）
 *      parameter_FK[0..2] = x_current, y_current, z_current (单位：μm)
 *      parameter_FK[3..5] = alpha_current, beta_current, gamma_current
(单位：°)
 *
 *  - x, y, z: 末端目标位置（单位：μm）
 *  - rx, ry, rz: 末端目标姿态（单位：°）
 *
 * 输出：
 *  - pulse_* : 各轴脉冲（int）
 *
 * 返回：
 *  - IK_SUCCESS (0) 或 IK_ERR_OUT_OF_RANGE (1)
 */
int incremental_inverse_kinematics(const double parameter[6],
                                   const double parameter_FK[6], double x_um,
                                   double y_um, double z_um, double rx_deg,
                                   double ry_deg, double rz_deg,
                                   int *pulse_theta1, int *pulse_d2,
                                   int *pulse_d3, int *pulse_d4,
                                   int *pulse_theta5, int *pulse_theta6);

#endif // INVERSE_KINEMATICS_H